// 条码检测距离：10-17cm
// 二维码检测距离：19-23cm

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include "opencv2/opencv.hpp"
#include <zbar.h> // zbar库 sudo apt-get install libzbar0 libzbar-dev
#include <iostream>
#include <string>
#include <vector>
#include <chrono>
#include <thread>

using namespace std;
using namespace cv;

double time_between_tolerance = 3.0; // 距离上次检测到的时间后没有检测到同样条码的最大间隔 s
double time_last = 0.0;              // 上次检测时间

std::string bar_info = "";      // 条形码信息
int is_bar_found_confirmed = 0; // CODE-128是否检测到标志
int is_bar_found_cnt = 0;       // CODE-128检测到的次数
int is_bar_found_cnt_max = 3;   // CODE-128检测到的最大次数
int barcode_count = 0;          // 条形码计数器
int barcode_count_max = 3;      // 条形码最大计数

std::string qr_info = "";      // QR-Code信息
int is_qr_found_confirmed = 0; // QR-Code是否检测到标志
int is_qr_found_cnt = 0;       // QR-Code检测到的次数
int is_qr_found_cnt_max = 3;   // QR-Code检测到的最大次数
int qr_count = 0;              // QR-Code计数器
int qr_count_max = 3;          // QR-Code最大计数

double ground_truth_x_where_last_shot = 0.0;
double ground_truth_y_where_last_shot = 0.0;
double ground_truth_x = 0.0;
double ground_truth_y = 0.0;
double stick_truth_x = 0.0;
double stick_truth_y = 0.0;

cv::Mat current_frame;
bool new_frame = false;

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        current_frame = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
        new_frame = true;
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }
}



void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    ground_truth_x = msg->pose.pose.position.x + stick_truth_x;
    ground_truth_y = msg->pose.pose.position.y + stick_truth_y;
}

void stickCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    stick_truth_x = msg->pose.position.x;
    stick_truth_y = msg->pose.position.y;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "qr_code_scanner");
    ros::NodeHandle nh;

    ros::Subscriber image_sub = nh.subscribe("/usb_cam/image_raw", 1, imageCallback);
    ros::Subscriber location_sub = nh.subscribe("/Odometry", 1, odomCallback);
    ros::Subscriber stick_location_sub = nh.subscribe("/stick_location", 1, stickCallback);
    ros::Publisher hold_pub = nh.advertise<geometry_msgs::PoseStamped>("/hold", 10);

    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

    ros::Rate loop_rate(30);

    while (ros::ok())
    {
        ros::spinOnce(); // 处理回调

            cv::Mat frame = current_frame.clone();
            if (frame.empty())
            {
                ROS_ERROR("Empty frame");
                loop_rate.sleep();
                continue;
            }

            if (1)
            {
                Mat hsv, mask_red1, mask_red2, mask_red, mask_yellow, mask_green, mask_blue;
                cvtColor(frame, hsv, COLOR_BGR2HSV);
                // Range [H, S, V]: MIN - [20, 119, 108], MAX - [34, 255, 220]
                Scalar lower_yellow(20, 119, 108);
                Scalar upper_yellow(34, 255, 220);
                inRange(hsv, lower_yellow, upper_yellow, mask_yellow);
                
                // 对黄色掩模进行两次腐蚀操作
                Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
                erode(mask_yellow, mask_yellow, element);
                erode(mask_yellow, mask_yellow, element);
                // 进行一次膨胀操作
                dilate(mask_yellow, mask_yellow, element);

                // 寻找轮廓
                vector<vector<Point>> contours;
                vector<Vec4i> hierarchy;
                findContours(mask_yellow, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

                // 寻找最大轮廓
                int maxContourIdx = -1;
                double maxContourArea = 0;
                for (int i = 0; i < contours.size(); i++) {
                    double area = contourArea(contours[i]);
                    if (area > maxContourArea) {
                        maxContourArea = area;
                        maxContourIdx = i;
                    }
                }

                // 如果找到轮廓，绘制并处理
                if (maxContourIdx >= 0) {
                    // 绘制轮廓
                    drawContours(frame, contours, maxContourIdx, Scalar(0, 255, 0), 2);
                    // 当轮廓面积大于150像素时进行拍照
                    if (maxContourArea > 500) {
                        // 如果barcode_count不为0，记录ground_truth_y
                        ground_truth_y_where_last_shot = ground_truth_y;
                        // 拍照并保存
                        std::string filename = "/home/firefly/temp/" + std::to_string(barcode_count) + ".jpg";
                        cv::imwrite(filename, frame);
                        
                        // 增加barcode_count计数
                        barcode_count++;
                        
                        ROS_INFO("检测到黄色物体，已拍照，barcode_count=%d", barcode_count);
                    }
                }

                // 显示掩模图像
                cv::imshow("Yellow Mask", mask_yellow);

                if (ros::Time::now().toSec() - time_last > time_between_tolerance)
                {
                    is_bar_found_cnt = 0;
                    is_bar_found_confirmed = 0;
                    is_qr_found_cnt = 0;
                    is_qr_found_confirmed = 0;
                }

                geometry_msgs::PoseStamped hold_msg;
                if ((is_bar_found_cnt && barcode_count < barcode_count_max) ||
                    (is_qr_found_cnt && qr_count < qr_count_max) ||
                    ros::Time::now().toSec() - time_last < time_between_tolerance)
                {
                    hold_msg.pose.orientation.w = 1.0;
                }
                else
                {
                    hold_msg.pose.orientation.w = 0.0;
                }
                cout << "is_hold: " << hold_msg.pose.orientation.w << endl;
                hold_pub.publish(hold_msg);

                cv::imshow("Camera Feed", frame);
                if (cv::waitKey(1) == 27)
                {
                    break;
                }
            }

            loop_rate.sleep();


        // return 0;
    }
}